/**
AirwayPathfinder for X-Plane Flight Simulator. Calculates the shortest path between two NavAids. 
Copyright (C) 2011  Nils Buletta

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 **/

package de.buletta.airwaypathfinder;

import java.util.HashMap;
import java.util.ArrayList;
import java.util.Iterator;
import java.util.PriorityQueue;

import org.jdesktop.swingx.mapviewer.GeoPosition;

/**
 * Provides the method to search for the shortest connection between to navaids
 * using the A* search algorithm.
 * 
 * @author Nils Buletta
 * @version 0.1
 */
public class Pathfinder {

	/**
	 * Searches for a shortest path between to {@link EarthNav} objects.
	 * 
	 * @param start
	 *            the {@link EarthNav} object to start the search from
	 * @param goal
	 *            the {@link EarthNav} object to reach
	 * @param layer
	 *            int - the layer to consider during the search. 1 for lower
	 *            layer, 2 for upper layer, 3 for both layers
	 * @param navs
	 *            the HashMap containing the navaids
	 * @return an {@link ArrayList} containing the EarthNav objects that form
	 *         the path. If no path was computed the ArrayList is empty
	 */
	public static ArrayList<EarthNav> calculatePath(EarthNav start,
			EarthNav goal, int layer, HashMap<String, ArrayList<EarthNav>> navs) {
		PriorityQueue<EarthNav> openList = new PriorityQueue<>();
		ArrayList<EarthNav> closedList = new ArrayList<>();
		EarthNav currentNav = null;
		boolean success = false;

		// get copys of the start and goal EarthNav objects to keep the navs
		// HashMap intact
		EarthNav searchStart = start.getSearchCopy();
		EarthNav searchGoal = goal.getSearchCopy();

		// set cost from start to start to 0
		searchStart.setEstimatedCost(computeDistance(searchStart.getPosition(),
				searchGoal.getPosition()));
		searchStart.setMoveCost(0);

		// insert start point into openlist
		openList.add(searchStart);

		while (!openList.isEmpty()) {

			// get point with the lowest cost
			currentNav = openList.poll();

			if (currentNav.equals(searchGoal)) {
				success = true;
				break;
			}

			// compute all sucessors of the current point
			expandNode(currentNav, searchGoal, closedList, openList, layer,
					navs);

			// everything computed for the current point. add to closed list
			closedList.add(currentNav);
		}
		ArrayList<EarthNav> result = new ArrayList<>();
		if (success) {
			EarthNav temp = searchGoal;
			while (temp.getPredecessor() != null) {
				result.add(0, temp);
				temp = temp.getPredecessor();
			}
			result.add(0, searchStart);
		}
		return result;

	}

	/**
	 * @deprecated <p>
	 *             Searches for a shortest path between to {@link EarthNav}
	 *             objects.
	 * 
	 * @param start
	 *            the {@link EarthNav} object to start the search from
	 * @param goal
	 *            the {@link EarthNav} object to reach
	 * @param layer
	 *            int - the layer to consider during the search. 1 for lower
	 *            layer, 2 for upper layer, 3 for both layers
	 * @param navs
	 *            the HashMap containing the navaids
	 * @return true if a path was found; false otherwise
	 */
	public static boolean calculatePath2(EarthNav start, EarthNav goal,
			int layer, HashMap<String, ArrayList<EarthNav>> navs) {
		PriorityQueue<EarthNav> openList = new PriorityQueue<>();
		ArrayList<EarthNav> closedList = new ArrayList<>();
		EarthNav currentNav = null;

		// get copys of the start and goal EarthNav objects to keep the navs
		// HashMap intact

		// set cost from start to start to 0
		start.setEstimatedCost(computeDistance(start.getPosition(),
				goal.getPosition()));
		start.setMoveCost(0);

		// insert start point into openlist
		openList.add(start);

		while (!openList.isEmpty()) {

			// get point with the lowest cost
			currentNav = openList.poll();

			if (currentNav.equals(goal)) {
				break;
			}

			// compute all sucessors of the current point
			expandNode2(currentNav, goal, closedList, openList, layer, navs);

			// everything computed for the current point. add to closed list
			closedList.add(currentNav);
		}

		return false;
	}

	private static void expandNode2(EarthNav currentNav, EarthNav goal,
			ArrayList<EarthNav> closedList, PriorityQueue<EarthNav> openList,
			int layer, HashMap<String, ArrayList<EarthNav>> navs) {
		EarthNav successor = null;
		for (Connection c : currentNav.getConnections()) {

			// filter connections by layer
			if (layer != 3) {
				if (c.getLayer() != 3) {
					if (c.getLayer() != layer) {
						continue;
					}
				}
			}

			successor = null;

			for (EarthNav n : navs.get(c.getIdentifier())) {
				if (c.getPosition().equals(n.getPosition())) {
					successor = n;
				}
			}

			if (successor == null)
				return;

			if (!closedList.contains(successor)) {

				// compute move cost from current point to successor
				double moveCost = currentNav.getMoveCost()
						+ computeDistance(currentNav.getPosition(),
								successor.getPosition());

				// if openlist contains successor and move cost are not less
				// than already compute cost do nothing
				// if (openList.contains(successor)) {
				// if (moveCost >= successor.getMoveCost()) {
				// continue;
				// }
				// }
				if (moveCost >= successor.getMoveCost()
						&& openList.contains(successor)) {
					continue;
				}

				successor.setPredecessor(currentNav);
				successor.setMoveCost(moveCost);
				successor.setEstimatedCost(computeDistance(
						successor.getPosition(), goal.getPosition()));
				if (openList.contains(successor)) {
					openList.remove(successor);
				}

				openList.add(successor);

			}
		}
	}

	private static void expandNode(EarthNav currentNav, EarthNav goal,
			ArrayList<EarthNav> closedList, PriorityQueue<EarthNav> openList,
			int layer, HashMap<String, ArrayList<EarthNav>> navs) {

		EarthNav successor = null;
		for (Connection c : currentNav.getConnections()) {

			// filter connections by layer
			if (layer != 3) {
				if (c.getLayer() != 3) {
					if (c.getLayer() != layer) {
						continue;
					}
				}
			}

			successor = null;

			for (EarthNav n : navs.get(c.getIdentifier())) {
				if (c.getPosition().equals(n.getPosition())) {
					// get a copy of the successor to keep the EarthNav HashMap
					// intact
					// if the successor equals our goal use the goal object
					// since it is already a copy
					if (n.equals(goal)) {
						successor = goal;
					} else {
						successor = n.getSearchCopy();
					}
				}
			}

			if (successor == null)
				return;

			if (!closedList.contains(successor)) {

				// compute move cost from current point to successor
				double moveCost = currentNav.getMoveCost()
						+ computeDistance(currentNav.getPosition(),
								successor.getPosition());

				// If the successor is already in the openList and its moveCost
				// are less than the newly computed moveCost do nothing and
				// continue with the next EarthNav.
				boolean cont = false;
				if (openList.contains(successor)) {
					// Because the successor object is a copy with moveCost = 0
					// we
					// have to get the already copied object from the openList
					Iterator<EarthNav> it = openList.iterator();
					while (it.hasNext()) {
						EarthNav n = it.next();
						if (n.equals(successor)) {
							if (moveCost >= n.getMoveCost()) {
								cont = true;
								break;
							}
						}
					}
				}
				if (cont == true)
					continue;

				// if (moveCost >= successor.getMoveCost()
				// && openList.contains(successor)) {
				// continue;
				// }

				successor.setPredecessor(currentNav);
				successor.setMoveCost(moveCost);
				successor.setEstimatedCost(computeDistance(
						successor.getPosition(), goal.getPosition()));
				if (openList.contains(successor)) {
					openList.remove(successor);
				}

				openList.add(successor);
			}
		}
	}

	/**
	 * Computes the distance between to geo coordinates.
	 * 
	 * @param pos1
	 *            the first geo coordinate
	 * @param pos2
	 *            the second geo coordinate
	 * @return the distance in Kilometers
	 */
	public static double computeDistance(GeoPosition pos1, GeoPosition pos2) {
		return 6378.388 * Math
				.acos(Math.sin(Math.toRadians(pos1.getLatitude()))
						* Math.sin(Math.toRadians(pos2.getLatitude()))
						+ Math.cos(Math.toRadians(pos1.getLatitude()))
						* Math.cos(Math.toRadians(pos2.getLatitude()))
						* Math.cos(Math.toRadians(pos2.getLongitude())
								- Math.toRadians(pos1.getLongitude())));
	}

	/**
	 * @deprecated <p>
	 *             Removes the predecessors from all {@link EarthNav} objects in
	 *             the navs HashMap.
	 *             <p>
	 *             This is only necessary if the
	 *             {@link Pathfinder#calculatePath2(EarthNav, EarthNav, int, HashMap)}
	 *             method was used to calculate a path
	 * @param navs
	 *            the {@code HashMap} containing the {@code EarthNav} objects
	 */
	public static void cleanNavs(HashMap<String, ArrayList<EarthNav>> navs) {
		for (ArrayList<EarthNav> a : navs.values()) {
			for (int i = 0; i < a.size(); i++) {
				a.get(i).setPredecessor(null);
				a.get(i).setMoveCost(0);
				a.get(i).setEstimatedCost(0);
			}
		}
	}
}
